#include <ros/ros.h> //ros头文件
#include <cv_bridge/cv_bridge.h> //图像转化头文件
#include <sensor_msgs/image_encodings.h> //图像编码格式头文件
#include <opencv2/imgproc/imgproc.hpp> //图像处理头文件
#include <opencv2/highgui/highgui.hpp> //图像化显示头文件
#include <opencv2/objdetect/objdetect.hpp> //检测函数头文件

using namespace cv;
using namespace std;

static CascadeClassifier face_cascade;//定义一个分类器，用来识别人脸

static Mat frame_gray; //相机彩色图像转为黑白图像
static vector<Rect> faces; //检测了多少人脸会存在这里
static vector<Rect>::const_iterator face_iter; //一个迭代器和faces对应

void callbackRGB(const sensor_msgs::Image msg)
{
    //数据转和数据存储
    cv_bridge::CvImagePtr cv_ptr;//图像类型指针
    try//保护程序，出现错误可以返回对应错误，保证程序不崩
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);//图像格式转化
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge excpption: %s", e.what());
        return;
    }

    Mat imgOriginal = cv_ptr->image;//创建一个opcv的格式图像信息

    //转化成黑白图像
    cvtColor(imgOriginal, frame_gray, CV_BGR2GRAY);
    equalizeHist(frame_gray,frame_gray);//均衡化

    //检测人脸
    face_cascade.detectMultiScale(frame_gray, faces, 1.1,3,0|CASCADE_SCALE_IMAGE, Size(30,30));

    //在彩色原图中标注人脸位置
    if(faces.size() > 0)
    {
        for(face_iter = faces.begin();face_iter != faces.end(); ++face_iter)
        {
            rectangle(
                imgOriginal,
                Point(face_iter->x , face_iter->y),
                Point(face_iter->x + face_iter->width, face_iter->y + face_iter->height),
                CV_RGB(255, 0, 255),
                2);

        }
    }
    imshow("faces", imgOriginal);
    waitKey(1);

}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "cv_face_detect");

    namedWindow("faces");

    std::string strLoadFile;
    char const* home = getenv("HOME");
    strLoadFile = home;
    strLoadFile += "/yall/cpp";
    strLoadFile += "/src/wpr_simulation/config/haarcascade_frontalface_alt.xml";

    bool res = face_cascade.load(strLoadFile);
	if (res == false)
	{
		ROS_ERROR("fail to load haarcascade_frontalface_alt.xml");
        return 0;
	}

    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, callbackRGB);

    ros::spin();
    return 0;
}